/*********************************************************************
* Rice University Software Distribution License
*
* Copyright (c) 2010, Rice University
* All Rights Reserved.
*
* For a full description see the file named LICENSE.
*
*********************************************************************/

/* Author: Ioan Sucan */

#include "omplapp/graphics/detail/RenderPlannerData.h"
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/base/spaces/SE3StateSpace.h>

#ifdef __APPLE__
#include <OpenGL/gl.h>
#else
#include <GL/gl.h>
#endif

namespace ompl
{
    namespace app
    {

        static void renderState(const base::SE2StateSpace::StateType &state)
        {
            glVertex3d(state.getX(), state.getY(), 0.0);
        }

        static void renderState(const base::SE3StateSpace::StateType &state)
        {
            glVertex3d(state.getX(), state.getY(), state.getZ());
        }
        
        static void renderEdge( const base::SE2StateSpace::StateType &stateA, const base::SE2StateSpace::StateType &stateB )
        {
            glVertex3d( stateA.getX(), stateA.getY(), -0.5 );
            glVertex3d( stateB.getX(), stateB.getY(), -0.5 );
        }

        static void renderEdge( const base::SE3StateSpace::StateType &stateA, const base::SE3StateSpace::StateType &stateB )
        {
            glVertex3d( stateA.getX(), stateA.getY(), stateA.getZ() );
            glVertex3d( stateB.getX(), stateB.getY(), stateB.getZ() );
        }

        static void setStateColor(int tag)
        {
            static const int NC = 7;
            static float colors[NC][4] =
                {
                    {1.0f, 0.0f, 0.0f, 0.6f},
                    {0.0f, 1.0f, 0.0f, 0.6f},
                    {0.0f, 0.0f, 1.0f, 0.6f},
                    {1.0f, 1.0f, 0.0f, 0.6f},
                    {0.0f, 1.0f, 1.0f, 0.6f},
                    {1.0f, 0.0f, 1.0f, 0.6f},
                    {1.0f, 1.0f, 1.0f, 0.6f},
                };

            int c = abs(tag) % NC;
            glColor4f(colors[c][0], colors[c][1], colors[c][2], colors[c][3]);
        }

        int RenderPlannerData(const base::PlannerData &pd, const aiVector3D &translate, MotionModel m, const GeometricStateExtractor &gse, unsigned int count)
        {
            int result = glGenLists(1);
            glNewList(result, GL_COMPILE);

            aiMatrix4x4 t;
            aiMatrix4x4::Translation(-translate, t);
            aiTransposeMatrix4(&t);
            glPushMatrix();
            glMultMatrixf((float*)&t);

            glDisable(GL_LIGHTING);
            glDisable(GL_COLOR_MATERIAL);

            glPointSize(3.5f);

            glBegin(GL_POINTS);

            if (m == Motion_2D)
                for (std::size_t i = 0; i < pd.numVertices(); ++i)
                {
                    const base::PlannerDataVertex& vtx = pd.getVertex(i);
                    setStateColor(vtx.getTag());
                    for (unsigned int r = 0 ; r < count ; ++r)
                    {
                        const base::State *st = gse(vtx.getState(), r);
                        const base::SE2StateSpace::StateType* se2st = static_cast<const base::SE2StateSpace::StateType*>(st);
                        renderState (*se2st);
                    }
                }
            else
                for (std::size_t i = 0; i < pd.numVertices(); ++i)
                {
                    const base::PlannerDataVertex& vtx = pd.getVertex(i);
                    setStateColor(vtx.getTag());
                    for (unsigned int r = 0 ; r < count ; ++r)
                    {
                        const base::State *st = gse(vtx.getState(), r);
                        const base::SE3StateSpace::StateType* se3st = static_cast<const base::SE3StateSpace::StateType*>(st);
                        renderState (*se3st);
                    }
                }

            glEnd();
            
            //Dobson : Draw the edges of the structure
            glColor4f( 1.0f, 1.0f, 1.0f, 0.6f );
            glLineWidth( 0.3f );
            
            glBegin(GL_LINES);
            if( m == Motion_2D )
                for( std::size_t i = 0; i < pd.numVertices(); ++i )
                {
                    for( std::size_t j = i+1; j < pd.numVertices(); ++j )
                    {
                        if( pd.edgeExists( i, j ) || pd.edgeExists( j, i ) )
                        {
                            for( unsigned int r = 0; r < count; ++r )
                            {
                                //Extract the state information
                                const base::State *sta = gse(pd.getVertex(i).getState(), r);
                                const base::SE2StateSpace::StateType* se2sta = static_cast<const base::SE2StateSpace::StateType*>(sta);
                                const base::State *stb = gse(pd.getVertex(j).getState(), r);
                                const base::SE2StateSpace::StateType* se2stb = static_cast<const base::SE2StateSpace::StateType*>(stb);
                                //Render the edge
                                renderEdge( *se2sta, *se2stb );
                            }
                        }
                    }
                }
            else
                for( std::size_t i = 0; i < pd.numVertices(); ++i )
                {
                    for( std::size_t j = i+1; j < pd.numVertices(); ++j )
                    {
                        if( pd.edgeExists( i, j ) || pd.edgeExists( j, i ) )
                        {
                            for( unsigned int r = 0; r < count; ++r )
                            {
                                //Extract the state information
                                const base::State *sta = gse(pd.getVertex(i).getState(), r);
                                const base::SE3StateSpace::StateType* se3sta = static_cast<const base::SE3StateSpace::StateType*>(sta);
                                const base::State *stb = gse(pd.getVertex(j).getState(), r);
                                const base::SE3StateSpace::StateType* se3stb = static_cast<const base::SE3StateSpace::StateType*>(stb);
                                //Render the edge
                                renderEdge( *se3sta, *se3stb );
                            }
                        }
                    }
                }
            glEnd();

            glPopMatrix();

            glEndList();
            return result;
        }

    }
}
